/**
 * 2024/03/15
 * ssyyw
 * mavproxy_ros1的飞行控制节点，根据当前飞行计划或指令，输出飞行控制命令到mavros
 *
 */

#include "flight_tool.h"
#include "flight_controller.h"

FlightController::FlightController()
    : nh()
{
    drone_state_sub = nh.subscribe<mavros_msgs::State>(MAVROS_INFO_STATE, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                       std::bind(&FlightController::StateCallback, this, std::placeholders::_1));
    drone_current_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>(MAVROS_INFO_LOCAL_POSE, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                     std::bind(&FlightController::PoseCallback, this, std::placeholders::_1));
    drone_current_gpspos_sub = nh.subscribe<sensor_msgs::NavSatFix>(MAVROS_INFO_GPS_POS, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                    std::bind(&FlightController::GPSCallback, this, std::placeholders::_1));
    drone_current_velocity_sub = nh.subscribe<geometry_msgs::TwistStamped>(MAVROS_INFO_VELOCITY, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                           std::bind(&FlightController::VelocityCallback, this, std::placeholders::_1));
    drone_flight_cmd_sub = nh.subscribe<mavproxy_ros1::FlightCmd>(MAVPROXY_PARSED_CMD, GLOBAL_DEFAULT_QUEUE_SIZE,
                                                                  std::bind(&FlightController::CmdCallback, this, std::placeholders::_1));

    drone_target_pose_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVROS_SET_LOCAL_POSE, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_target_gpose_pub = nh.advertise<geographic_msgs::GeoPoseStamped>(MAVROS_SET_GLOABL_POSE, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_target_velocity_pub = nh.advertise<geometry_msgs::Twist>(MAVROS_SET_VELOCITY, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_target_attitude_pub = nh.advertise<geometry_msgs::PoseStamped>(MAVROS_SET_ATTITUDE, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_current_flight_state_pub = nh.advertise<mavproxy_ros1::FlightState>(MAVPROXY_FLIGHT_STATE, GLOBAL_DEFAULT_QUEUE_SIZE);
    drone_target_goal_pub = nh.advertise<geometry_msgs::PoseStamped>(LOCAL_PLANNER_GOAL_POSITION, GLOBAL_DEFAULT_QUEUE_SIZE);

    drone_arming_client = nh.serviceClient<mavros_msgs::CommandBool>(MAVROS_SRV_ARM);
    drone_set_home_client = nh.serviceClient<mavros_msgs::CommandHome>(MAVROS_SET_HOME);
    drone_mode_client = nh.serviceClient<mavros_msgs::SetMode>(MAVROS_SRV_MODE);
    drone_takeoff_cmd_client = nh.serviceClient<mavros_msgs::CommandTOL>(MAVROS_SRV_TAKEOFF);
    drone_land_cmd_client = nh.serviceClient<mavros_msgs::CommandTOL>(MAVROS_SRV_LAND);

    service = nh.advertiseService(MAVPROXY_CONTROLL, std::bind(&FlightController::RunReceiveCmd, this, std::placeholders::_1, std::placeholders::_2));

    cmdExecuters = {
        {CMD_FLIGHT_TAKEOFF, std::bind(&FlightController::LetDroneTakeoff, this)},
        {CMD_FLIGHT_LAND, std::bind(&FlightController::LetDroneLand, this)},
        {CMD_FLIGHT_HOVER, std::bind(&FlightController::LetDroneHover, this)},
        {CMD_FLIGHT_FLY_POSE, std::bind(&FlightController::LetDroneFlyToTargetPosition, this)},
        {CMD_FLIGHT_FLY_GPOSE, std::bind(&FlightController::LetDroneFlyToTargetGpsPosition, this)},
        {CMD_FLIGHT_ROTATEH, std::bind(&FlightController::LetDroneRotate, this)},
        {CMD_FLIGHT_RETURN, std::bind(&FlightController::LetDroneReturn, this)},
        {CMD_FLIGHT_ARM, std::bind(&FlightController::LetDroneReturn, this, true)},
        {CMD_FLIGHT_DISARM, std::bind(&FlightController::LetDroneReturn, this, false)},
        {CMD_FLIGHT_STOP, std::bind(&FlightController::LetDroneStop, this)}};
}

void FlightController::StateCallback(const mavros_msgs::State::ConstPtr &msg)
{
    drone_current_state = *msg;

    if (drone_current_state.armed == true)
    {
        ROS_DEBUG("Drone is armd");
    }
    else
    {
        ROS_DEBUG("Drone is disarmd");
    }
}

void FlightController::PoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    drone_current_pose = *msg;
}

void FlightController::VelocityCallback(const geometry_msgs::TwistStamped::ConstPtr &msg)
{
    drone_current_velocity = *msg;
}

void FlightController::CmdCallback(const mavproxy_ros1::FlightCmd::ConstPtr &msg)
{
    ROS_INFO("Cmd get %s", (*msg).cmdtype.c_str());
    drone_current_cmd = *msg;

    int ret = HandleCmd();
    if (ret != RETSUCCESS)
    {
        ROS_ERROR("HandleCmd failed");
    }
    else
    {
        ROS_INFO("HandleCmd success");
    }
}

void FlightController::GPSCallback(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
    drone_current_gpose.pose.position.latitude = (*msg).latitude;
    drone_current_gpose.pose.position.longitude = (*msg).longitude;
    drone_current_gpose.pose.position.altitude = (*msg).altitude;
}

int FlightController::SwitcheModeToTarget(const std::string &mode_name)
{
    mavros_msgs::SetMode srv;
    srv.request.custom_mode = mode_name;

    if (drone_mode_client.call(srv) && srv.response.mode_sent)
    {
        ROS_INFO("Drone mode set to %s", mode_name.c_str());
        return RETSUCCESS;
    }
    else
    {
        ROS_ERROR("Failed to set drone mode to %s", mode_name.c_str());
        return RETFAILED;
    }
}

int FlightController::SwitcheModeToTarget()
{
    if (drone_current_cmd.paramdata.size() < 1)
    {
        ROS_ERROR("Failed to set drone mode, because the mode id not send");
        return RETFAILED;
    }

    mavros_msgs::SetMode srv;
    srv.request.custom_mode = drone_current_cmd.paramdata[0];

    if (drone_mode_client.call(srv) && srv.response.mode_sent)
    {
        ROS_INFO("Drone mode set to %s", mode_name.c_str());
        return RETSUCCESS;
    }
    else
    {
        ROS_ERROR("Failed to set drone mode to %s", mode_name.c_str());
        return RETFAILED;
    }
}

int FlightController::ArmDrone(bool arm)
{
    mavros_msgs::CommandBool srv;
    srv.request.value = arm;

    // ros::service::waitForService(MAVROS_SRV_ARM);
    if (drone_arming_client.call(srv) && srv.response.success)
    {
        ROS_INFO("Drone %sarmed", arm ? "" : "dis");
        return RETSUCCESS;
    }
    else
    {
        ROS_ERROR("Failed to %sarm drone", arm ? "" : "dis");
        return RETFAILED;
    }
}

int FlightController::ArmAndCheckoutMode(const std::string &target_mode)
{
    ros::Rate rate(GLOBAL_COMMON_ROS_RATE);

    mavros_msgs::SetMode set_mode_message;
    set_mode_message.request.custom_mode = target_mode;

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();
    while (ros::ok())
    {
        if (!drone_current_state.armed && (ros::Time::now() - last_request > ros::Duration(GLOBAL_ENABLE_MESSAGE_DURATION)))
        {
            ArmDrone(true);
            last_request = ros::Time::now();
        }

        if (ros::Time::now() - last_request > ros::Duration(GLOBAL_ENABLE_MESSAGE_DURATION))
        {
            break;
        }

        ros::spinOnce();
        rate.sleep();
    }
    return RETSUCCESS;
}

int FlightController::GetTargetLocalPosition()
{
    if (drone_current_cmd.paramdata.size() >= 3)
    {
        drone_target_pose.pose.position.x = drone_current_cmd.paramdata[0];
        drone_target_pose.pose.position.y = drone_current_cmd.paramdata[1];
        drone_target_pose.pose.position.z = drone_current_cmd.paramdata[2];
    }
    else
    {
        ROS_INFO("Failed to get target local position, error param size [%ld].", drone_current_cmd.paramdata.size());
        return RETFAILED;
    }

    return RETSUCCESS;
}

int FlightController::GetTargetPlannerGoal()
{
    if (drone_current_cmd.paramdata.size() >= 3)
    {
        drone_target_goal.pose.position.x = drone_current_cmd.paramdata[0];
        drone_target_goal.pose.position.y = drone_current_cmd.paramdata[1];
        drone_target_goal.pose.position.z = drone_current_cmd.paramdata[2];
        drone_target_goal.header.frame_id = "local_origin";
    }
    else
    {
        ROS_INFO("Failed to get target local position, error param size [%ld].", drone_current_cmd.paramdata.size());
        return RETFAILED;
    }

    return RETSUCCESS;
}

int FlightController::GetTargetGpsPosition()
{
    if (drone_current_cmd.paramdata.size() >= 3)
    {
        drone_target_gpose.pose.position.latitude = drone_current_cmd.paramdata[0];
        drone_target_gpose.pose.position.longitude = drone_current_cmd.paramdata[1];
        drone_target_gpose.pose.position.altitude = drone_current_cmd.paramdata[2];
    }
    else
    {
        ROS_WARN("Failed to get target gps position, error param.");
        return RETFAILED;
    }

    drone_target_gpose.pose.orientation.x = 0;
    drone_target_gpose.pose.orientation.y = 0;
    drone_target_gpose.pose.orientation.z = 0;
    drone_target_gpose.pose.orientation.w = 0;

    return RETSUCCESS;
}

int FlightController::LetDroneTakeoff()
{
    ROS_INFO("Enter to let drone takeoff");
    // 确定当前为自稳状态
    if (drone_current_state.mode != STABLIZE_PX4_MODE)
    {
        ROS_WARN("Failed to handle takeoff task, current mode is not stablize");
        return RETFAILED;
    }

    // 在stablize模式下执行解锁命令，不管当前有没有解锁
    int ret = ArmDrone(true);
    if (ret != RETSUCCESS)
    {
        ROS_WARN("Failed to arm drone when takeoff.");
        return RETFAILED;
    }

    // 执行解锁成功后，切换到TAKEOFF模式触发起飞行为
    ret = SwitcheModeToTarget(OFFBOARD_PX4_MODE);
    if (ret != RETSUCCESS)
    {
        ROS_WARN("Failed to change mode when preparing takeoff.");
        return RETFAILED;
    }

    // controller仅下发起飞行为，不维护任何状态
    ROS_INFO("Send takeoff operation success.");
    return RETSUCCESS;
}

int FlightController::LetDroneLand()
{
    ROS_INFO("Enter to let drone land");
    // 确定当前为HOLD模式
    if (drone_current_state.mode != HOLD_PX4_MODE)
    {
        ROS_WARN("Failed to handle land task, current mode is not hold");
        return RETFAILED;
    }

    // 在HOLD模式下切换自动降落模式
    int ret = SwitcheModeToTarget(HOLD_PX4_MODE);
    if (ret != RETSUCCESS)
    {
        ROS_WARN("Failed to change mode when preparing land.");
        return RETFAILED;
    }

    // controller仅下发降落行为，不维护任何状态
    ROS_INFO("Send land operation success.");
    return RETSUCCESS;
}

// 默认的飞行命令就使能在OFFBOARD模式下
int FlightController::LetDroneFlyToTargetPosition()
{
    ROS_INFO("Enter to let drone fly to target position.");
    // 确定当前为HOLD模式
    if (drone_current_state.mode != HOLD_PX4_MODE)
    {
        ROS_WARN("Failed to handle fly to target task, current mode is not hold");
        return RETFAILED;
    }

    // 首先让无人机定点在当前位置，避免从HOLD模式切换到OFFBOARD模式后，因为先前命令没清除导致意外飞行
    drone_target_goal_pub.publish(drone_current_pose);

    // 获取给定的期望位置坐标
    int ret = GetTargetPlannerGoal();
    if (ret != RETSUCCESS)
    {
        ROS_INFO("Failed to get target local target position before flying to target position.");
        return RETFAILED;
    }

    // 切换OFFBOARD模式
    ret = SwitcheModeToTarget(OFFBOARD_PX4_MODE);
    if (ret != RETSUCCESS)
    {
        ROS_WARN("Failed to change mode when fly to target position.");
        return RETFAILED;
    }

    // TODO: 切到OFFBOARD后检查当前飞行状态，如异常，执行紧急切出到HOLD模式

    // 确认正常后下发期望位置给避障系统，由避障系统接管无人机控制
    drone_target_goal_pub.publish(drone_target_goal);

    return RETSUCCESS;
}

// TODO:是否要增补无避障状态下的飞往指定目标点的操作
int FlightController::LetDroneFlyToTargetPositionWithOutOBS()
{
    ROS_INFO("Enter to let drone fly to target position.");
    // 确定当前为HOLD模式
    if (drone_current_state.mode != HOLD_PX4_MODE)
    {
        ROS_WARN("Failed to handle fly to target task, current mode is not hold");
        return RETFAILED;
    }

    // 获取位置
    int ret = GetTargetPlannerGoal();
    if (ret != RETSUCCESS)
    {
        ROS_INFO("Failed to get target local position before flying to target position.");
        return RETFAILED;
    }

    drone_target_goal_pub.publish(drone_target_goal);
    return RETSUCCESS;
}

int FlightController::LetDroneFlyToTargetGpsPosition()
{
    ROS_INFO("Enter to let drone fly to target position.");

    // 确定当前为HOLD模式
    if (drone_current_state.mode != HOLD_PX4_MODE)
    {
        ROS_WARN("Failed to handle fly to target task, current mode is not hold");
        return RETFAILED;
    }

    // 首先让无人机定点在当前位置，避免从HOLD模式切换到OFFBOARD模式后，因为先前命令没清除导致意外飞行
    drone_target_goal_pub.publish(drone_current_pose);

    // 获取目标GPS位置，再将GPS坐标转换成本地坐标
    int ret = GetTargetGpsPosition();
    if (ret != RETSUCCESS)
    {
        ROS_INFO("Failed to get target gps position before flying to target position.");
        return RETFAILED;
    }

    // 切换OFFBOARD模式
    ret = SwitcheModeToTarget(OFFBOARD_PX4_MODE);
    if (ret != RETSUCCESS)
    {
        ROS_WARN("Failed to change mode when fly to target position.");
        return RETFAILED;
    }

    // TODO: 切到OFFBOARD后检查当前飞行状态，如异常，执行紧急切出到HOLD模式

    // 确认正常后下发期望位置给避障系统，由避障系统接管无人机控制
    drone_target_goal_pub.publish(drone_target_goal);

    ROS_INFO("Send fly to target gpos success");
    return RETSUCCESS;
}

int FlightController::LetDroneHover()
{
    int ret = RETFAILED;

    if (drone_current_state.mode == HOLD_PX4_MODE)
    {
        ROS_INFO("Current state is already hold mode, please check current state manualy.");
        ret = RETSUCCESS;
    }

    if (drone_current_state.mode == TAKEOFF_PX4_MODE || drone_current_state.mode == MISSION_PX4_MODE || drone_current_state.mode == LAND_PX4_MODE)
    {
        ret = SwitcheModeToTarget(HOLD_PX4_MODE);
        if (ret != RETSUCCESS)
        {
            ROS_ERROR("Failed to switch mode to LOITER when change the mode frome takeoff to hold.");
            ret = RETFAILED;
        }
        else
        {
            ret = RETSUCCESS;
        }
    }

    if (drone_current_state.mode == OFFBOARD_PX4_MODE)
    {
        // 将当前坐标设置为期望坐标
        drone_target_goal_pub.publish(drone_current_pose);

        // 停2s等待避障系统重新规划并飞行到期望位置
        sleep(2);

        // 将模式重新设置为HOLD
        ret = SwitcheModeToTarget(HOLD_PX4_MODE);
        if (ret != RETSUCCESS)
        {
            ROS_ERROR("Failed to switch mode to LOITER when change the mode frome takeoff to hold.");
            ret = RETFAILED;
        }
        else
        {
            ret = RETSUCCESS;
        }
    }

    ROS_INFO("Handle the hover cmd end");
    return ret;
}

// 让无人机旋转指定角度，当前未经验证，而且和避障存在冲突，避障会接管航向角控制导致旋转失效
int FlightController::LetDroneRotate()
{
    geometry_msgs::Twist twist;
    twist.linear.x = 0.0; // 线速度设置为0
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;

    // 角速度设置为顺时针旋转，转速为1.5弧/秒
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 1.5; // 负值表示逆时针旋转
    drone_target_velocity_pub.publish(twist);

    return RETSUCCESS;
}

int FlightController::LetDroneReturn()
{
    ROS_INFO("Enter to drone go back home.");

    // 确定当前为HOLD模式
    if (drone_current_state.mode != HOLD_PX4_MODE)
    {
        ROS_WARN("Failed to handle land task, current mode is not hold");
        return RETFAILED;
    }

    // 在HOLD模式下切换返航模式
    int ret = SwitcheModeToTarget(RETURN_PX4_MODE);
    if (ret != RETSUCCESS)
    {
        ROS_WARN("Failed to change mode when preparing land.");
        return RETFAILED;
    }

    // controller仅下发降落行为，不维护任何状态
    ROS_INFO("Send return operation success.");
    return RETSUCCESS;
}

// TODO: STOP
// 1、任务发到一半想停下来，不打算发其他命令的整体终止
// 2、出现飞行过高的场景，命令终止不了，直接切入命令，强制让无人机着陆
// 3、用封装的紧急停桨命令关闭桨叶
int FlightController::LetDroneStop()
{
    return RETSUCCESS;
}

// 让无人机设置home点，该位置依靠GPS，故需要在GPS正常时触发
int FlightController::LetDroneSetHome()
{
    ROS_INFO("Enter to set home.");

    // TODO：增加校验GPS状态判断，避免漂移过大导致返航出现问题

    // 通过GPS下发指定的home点
    mavros_msgs::CommandHome drone_target_home_point;
    drone_target_home_point.request.latitude = drone_current_gpose.pose.position.latitude;
    drone_target_home_point.request.longitude = drone_current_gpose.pose.position.longitude;
    drone_target_home_point.request.altitude = drone_current_gpose.pose.position.altitude;

    if (drone_set_home_client.call(drone_target_home_point) && drone_target_home_point.response.success)
    {
        ROS_INFO("Home position set successfully.");
        return RETSUCCESS;
    }

    ROS_ERROR("Failed to send set home position cmd.");
    return RETFAILED;
}

int FlightController::HandleCmd()
{
    ROS_INFO("Enter to handle command");

    // 寻找执行函数
    auto executer = cmdExecuters.find(drone_current_cmd.cmdtype);
    if (executer != cmdExecuters.end())
    {
        ROS_INFO("Find target command executer.");
    }
    else
    {
        ROS_ERROR("No handler for command type: %s", drone_current_cmd.cmdtype.c_str());
        return RETFAILED;
    }

    // 执行命令函数
    ROS_INFO("Start to execute the cmd [%s].", drone_current_cmd.cmdtype.c_str());
    int ret = executer->second();
    if (ret != RETSUCCESS)
    {
        ROS_INFO("Execute the cmd failed.");
        return RETFAILED;
    }

    return RETSUCCESS;
}

bool FlightController::RunReceiveCmd(mavproxy_ros1::FlightControll::Request &req, mavproxy_ros1::FlightControll::Response &res)
{
    res.result = RETFAILED;

    drone_current_cmd.cmdtype = req.cmdtype;
    drone_current_cmd.paramdata = req.paramdata;

    res.result = HandleCmd();

    return res.result;
}

void FlightController::InitilizeNode()
{
    // 等待飞控和MAVROS建立连接
    ros::Rate rate(GLOBAL_COMMON_ROS_RATE);

    ros::Time last_request = ros::Time::now();
    while (ros::ok() && !drone_current_state.connected && ros::Time::now() - last_request < ros::Duration(DRONE_CONNECTION_TIMEOUT_DURATION))
    {
        ros::spinOnce();
        rate.sleep();
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "flight_controller");
    FlightController node;
    node.InitilizeNode();
    ros::spin();
    return RETSUCCESS;
}
